{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "48c487ee",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 16: Advanced Visual Servoing"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b807377e",
   "metadata": {},
   "source": [
    "# 16.1 XY/Z-Partitioned IBVS\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2de74e7",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m IBVS-partitioned-main -H"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "65045197",
   "metadata": {},
   "source": [
    "# 16.2 IBVS Using Polar Coordinates\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98c72ea8",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default(pose=SE3.Tz(-2)*SE3.Rz(pi))\n",
    "P = mkgrid(2, 0.5, pose=SE3.Tz(2))\n",
    "ibvs = IBVS_polar(camera, lmbda=0.1, P=P, pose_d=SE3.Tz(1), depth=2, graphics=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8c989f4",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.run()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d289d484",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.plot_p()\n",
    "ibvs.plot_pose()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1059865e",
   "metadata": {},
   "source": [
    "# 16.3 IBVS for a Spherical Camera\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fecfc96c",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = SphericalCamera(pose=SE3.Trans(0.3, 0.3, -2)*SE3.Rz(0.4))\n",
    "P = mkgrid(2, side=1.5, pose=SE3.Tz(0.5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13bf992b",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs = IBVS_sph(camera, P=P, pose_d=SE3.Tz(-1.5), verbose=False, graphics=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cee4bfe4",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.run()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fcda91e8",
   "metadata": {},
   "source": [
    "# 16.4 Applications\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8680ee81",
   "metadata": {},
   "source": [
    "## 16.4.1 Arm-Type Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63a58d46",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m IBVS-arm-main -H"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d33b1f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(out.clock1.t, out.clock1.x)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ddeff16e",
   "metadata": {},
   "source": [
    "## 16.4.2 Mobile Robot\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0af346c",
   "metadata": {},
   "source": [
    "### 16.4.2.1 Holonomic Mobile Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2799687c",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default(f=0.002);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "86f56cde",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_B_C = SE3.Trans(0.2, 0.1, 0.3) * SE3.Rx(-pi/4);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1940814d",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = np.array([[0, 1, 2], [0, -1, 2]]).T;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2287f600",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m IBVS-holonomic-main -H"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7acfe4db",
   "metadata": {},
   "source": [
    "### 16.4.2.2 Nonholonomic Mobile Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eadd9d27",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m IBVS-nonholonomic-main -H"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5cdd16f7",
   "metadata": {},
   "source": [
    "## 16.4.3 Aerial Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "480778c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m IBVS-quadrotor-main -H"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "884dc97c",
   "metadata": {},
   "source": [
    "# 16.5 Wrapping Up\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1491dafd",
   "metadata": {},
   "source": [
    "## 16.5.1 Further Reading\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "804202aa",
   "metadata": {},
   "source": [
    "## 16.5.2 Resources\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d451a351",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "source": [
    "## 16.5.3 Exercises\n"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python",
   "version": "3.9.12 (main, Jun  1 2022, 06:34:44) \n[Clang 12.0.0 ]"
  },
  "vscode": {
   "interpreter": {
    "hash": "019dbbf6354d31390a1af2a8707908a74d3d4774daf62f20d47153cfbd4b4bc4"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
